import random

from robot.robot_env import Map, MapObject, MapAgent, MovingAgent, CollisionManager, PathPlanner, TerraNodes
import matplotlib.patches as patches
import matplotlib.pyplot as plt

from walle_event import SimpleEvent
from walle_priority_queue import WallePriorityQueue
from walle_world import WalleWorld


class SlamMap(Map):
    def __init__(self, map_id, width=1, height=1, carts=[], terra_width=1, terra_height=1):
        Map.__init__(self, map_id, width, height, [])
        self.carts = carts
        self.map = list()
        self.figure = plt.figure(figsize=(4, 8))
        self.ax = self.figure.add_subplot(1, 1, 1, aspect='equal')
        self.terra_width = terra_width
        self.terra_height = terra_height
        self.sleeping_nodes = []
        self.charging_nodes = []
        self.in_queue_nodes = []
        self.out_queue_nodes = []

    def load_map(self, filename):
        self.map = list()
        line_stack = list()
        with open(filename, "r") as f:
            for line in f:
                line_stack.append(line)
        line_stack.reverse()
        self.height = len(line_stack)
        for y in range(len(line_stack)):
            row = list()
            row_data = str.split(line_stack[y], ",")
            self.width = len(row_data)
            for x in range(len(row_data)):
                node = SlamMapTerraNode(x, y, row_data[x])
                row.append(node)
                if node.node_type == SlamMapTerraNode.AVG_SLEEPING:
                    self.sleeping_nodes.append(node)
                elif node.node_type == SlamMapTerraNode.AVG_CHARGING:
                    self.charging_nodes.append(node)
                elif node.node_type == SlamMapTerraNode.AVG_PICKING_IN or node.node_type == SlamMapTerraNode.AVG_WAITING_IN:
                    self.in_queue_nodes.append(node)
                elif node.node_type == SlamMapTerraNode.AVG_PICKING_OUT or node.node_type == SlamMapTerraNode.AVG_WAITING_OUT:
                    self.out_queue_nodes.append(node)
            self.map.append(row)
        self.ax.set_xlim(0, self.width * self.terra_width)
        self.ax.set_ylim(0, self.height * self.terra_height)

    def render(self, render_waiting=0.1, robots=[]):
        self.ax.cla()
        for row in self.map:
            for wp in row:
                self.ax.add_patch(wp.get_render_patches(self.terra_width, self.terra_height))
        for robot in robots:
            self.ax.add_patch(robot.get_render_patches(self.terra_width, self.terra_height))
        plt.pause(render_waiting)

    def get_node_by_id(self, id):
        cords = SlamMapTerraNode.id2point(id)
        return self.map[cords[1]][cords[0]]

    def get_node_by_xy(self, x, y):
        return self.map[y][x]

    def get_node_by_index(self, index):
        y = index / self.width
        x = index % self.width
        return self.map[y][x]

    @staticmethod
    def get_path_str(waypoint_list):
        res = ""
        for wp in waypoint_list:
            res += str(wp) + "->"
        return res


class SlamMapTerraNode(TerraNodes):
    AVG_SLEEPING = 1
    PATH = 6
    AVG_PICKING_IN = 2
    AVG_PICKING_OUT = 8
    AVG_WAITING_IN = 3
    AVG_WAITING_OUT = 7
    AVG_CHARGING = 4
    NO_ENTERANCE = 9

    @staticmethod
    def get_color(node_type):
        if node_type == SlamMapTerraNode.AVG_SLEEPING:
            return "#A59591"
        elif node_type == SlamMapTerraNode.AVG_PICKING_IN:
            return "#F08080"
        elif node_type == SlamMapTerraNode.AVG_PICKING_OUT:
            return "#00868B"
        elif node_type == SlamMapTerraNode.AVG_WAITING_IN:
            return "#EEAD0E"
        elif node_type == SlamMapTerraNode.PATH:
            return "#f1f1f1"
        elif node_type == SlamMapTerraNode.AVG_CHARGING:
            return "#B59591"
        elif node_type == SlamMapTerraNode.AVG_WAITING_OUT:
            return "#9BCD9B"
        elif node_type == SlamMapTerraNode.NO_ENTERANCE:
            return "#5D478B"
        else:
            return "#000000"

    def __init__(self, x, y, attribute):
        TerraNodes.__init__(self, "", x, y, "", attribute, [])
        self.node_id = SlamMapTerraNode.point2id(self.x, self.y)
        attribute_list = str.split(attribute, "|")
        try:
            type_str = attribute_list[0]
            # self.type = WayPointType(int(type_str))
            self.node_type = int(type_str)
            self.color = SlamMapTerraNode.get_color(int(type_str))
            if len(attribute_list) > 1:
                self.neighbours = [int(s) for s in str.split(attribute_list[1], ";")[:8]]
        except Exception, e:
            pass

    def __str__(self):
        return "(%d,%d)" % (self.x, self.y)

    def get_render_patches(self, sq_width, sq_height):
        return patches.Rectangle((self.x * sq_width, self.y * sq_height), sq_width, sq_height, fill=True,
                                 facecolor=self.color,
                                 edgecolor='gray', linewidth=0.5, linestyle="solid")

    @staticmethod
    def id2point(id):
        arr = str.split(id, ",")
        return int(arr[0]), int(arr[1])

    @staticmethod
    def point2id(x, y):
        return "%s,%s" % (x, y)

    def get_neighbours(self):
        neighbours_map = []
        x = self.x
        y = self.y
        for i in range(len(self.neighbours)):
            n_x = x
            n_y = y
            if self.neighbours[i] != 0:
                if i == 0:
                    n_x = x - 1
                    n_y = y + 1
                elif i == 1:
                    n_y = y + 1
                elif i == 2:
                    n_x = x + 1
                    n_y = y + 1
                elif i == 3:
                    n_x = x + 1
                elif i == 4:
                    n_x = x + 1
                    n_y = y - 1
                elif i == 5:
                    n_y = y - 1
                elif i == 6:
                    n_x = x - 1
                    n_y = y - 1
                else:
                    n_x = x - 1
                neighbours_map.append((n_x, n_y))

        return neighbours_map


class Cart(MapObject):
    def __int__(self, bucket_id, current_node):
        MapObject.__init__(self, bucket_id, current_node, station=None, agent=None)
        self.bucket_id = bucket_id
        self.station = None
        self.agent = None


